#include "DirectDriveRushed.h"
#include "Servo.h"
#include "LiquidCrystal.h"

#define PIN_SERVO_THROTTLE 9
#define DEGREE_THROTTLE_LOW 10
#define DEGREE_THROTTLE_HIGH 18
#define DELAY_THROTTLE_HIGH 80
#define PIN_SERVO_CLUTCH 10
#define DEGREE_CLUTCH_DOWN 150
#define DEGREE_CLUTCH_UP 180
float SECONDS_FULL_THROTTLE = 3.00;
#define SECONDS_IDLE 0
#define LOAD_WHEEL_MIN_RPM 100
#define BUTTON_DEBOUNCE 90

LiquidCrystal lcd( 12, 11, 5, 4, 3, 7 );

Servo throttle;
Servo clutch;

struct tach
{
  unsigned long nMillisLast;
  unsigned long nMillisCurrent;
};

struct tach tachLoadWheel;

void tachInit( struct tach &tach, int nMillisLastInit, int nMillisCurrentInit )
{
  tach.nMillisLast = nMillisLastInit;
  tach.nMillisCurrent = nMillisCurrentInit;
}

void tachISR()
{
  if ( ( millis() - tachLoadWheel.nMillisCurrent ) >= BUTTON_DEBOUNCE )
  {
    tachLoadWheel.nMillisLast = tachLoadWheel.nMillisCurrent;
    tachLoadWheel.nMillisCurrent = millis();
  }
}

int tachGetRpm( struct tach &tach )
{
  unsigned long nDiff = ( tach.nMillisCurrent - tach.nMillisLast );
  return ( 60000 / nDiff );
}

void setup()
{
  lcd.begin( 16, 2 );
  lcd.print( "Initializing..." );

  attachInterrupt( 0, tachISR, FALLING );
  digitalWrite( 2, HIGH );

  tachInit( tachLoadWheel, 0, 0 );

  throttle.attach( PIN_SERVO_THROTTLE );
  clutch.attach( PIN_SERVO_CLUTCH );

  throttle.write( DEGREE_THROTTLE_LOW );
  clutch.write( DEGREE_CLUTCH_UP );

  delay( 5000 );
  lcd.clear();
}

void loop()
{

  lcd.clear();
  lcd.print( "up and idle" );
  throttle.write( DEGREE_THROTTLE_LOW );
  clutch.write( DEGREE_CLUTCH_UP );
  delay( 3000 );

  lcd.clear();
  lcd.print( "down and idle" );
  clutch.write( DEGREE_CLUTCH_DOWN );
  delay( 3000 );

  lcd.clear();
  lcd.print( "keep idle" );
  delay( SECONDS_IDLE * 1000 );

  lcd.clear();
  lcd.print( "down and high" );
  for ( int nPos =  DEGREE_THROTTLE_LOW; nPos < DEGREE_THROTTLE_HIGH; ++nPos )
  {
    throttle.write( nPos );
    delay( DELAY_THROTTLE_HIGH );
  }

  lcd.clear();
  lcd.print( "keep high" );
  delay( SECONDS_FULL_THROTTLE * 1000 );

  lcd.clear();
  lcd.print( "up and idle" );
  throttle.write( DEGREE_THROTTLE_LOW );
  clutch.write( DEGREE_CLUTCH_UP );
  delay( 3000 );

  lcd.clear();
  lcd.print( "wait for slow" );
  delay( 2000 );
  while ( tachGetRpm( tachLoadWheel ) > LOAD_WHEEL_MIN_RPM )
  {
    if ( millis() % 10 == 0 )
    {
      lcd.clear();
       lcd.print("rpm: ");
      lcd.print( tachGetRpm( tachLoadWheel ) );
    }
  }

}

